#include "motion_control.h"
#include "cpp_log.h"


motion_control::motion_control(vector<Vector3d> path)
{
    foresee = 0.7;
    speed = 0.5;
    P = 1.0;//80;//40;
    D = 0.0;//25;
    a_b = 0.5;
    errLast = 0;
    tread = 0.3836;
    isRotation = false;
    riseRotation = 0.03;
    tracking_flag = false;
    this->path = path;
}

void motion_control::set_foresee (double foresee)
{
    this->foresee = foresee;
}

void motion_control::set_speed (double speed)
{
    this->speed = speed;
}

void motion_control::set_P (double P)
{
    this->P = P;
}

void motion_control::set_a_b(double a_b) {
    this->a_b = a_b;
    LOGD ("set a_b: %lf\n", this->a_b);
}

void motion_control::set_D (double D)
{
    this->D = D;
}

size_t motion_control::nearest (Vector3d p)
{
    assert (path.size()>0);
    size_t index = 0;
    double distance = 99999999.9;
    for (size_t i=0; i<path.size(); i++)
        if ((path[i]-p).norm()<distance) {
            distance = (path[i]-p).norm();
            index = i;
        }
    return index;
}

size_t motion_control::next (size_t index)
{
    index ++;
    if (index>=path.size())
        index = 0;
    return index;
}

//TODO: call twice, the scond slow.
Vector3d motion_control::aim_point (Vector3d pos, Vector3d dir)
{
    size_t index;
    double a = foresee;
    double b = a_b * a;  //0.5*a ;  //0.3*a;
    double c = sqrt(a*a-b*b);
    dir.normalize();
    Vector3d focus1 = pos + dir*c;
    Vector3d focus2 = pos - dir*c;

    if (tracking_flag==false) {
        tracking_flag = true;
        index = nearest (pos);
    } else if ((path[last_aim_index]-focus1).norm()+(path[last_aim_index]-focus2).norm()>4*a) {
        index = nearest (pos);
    } else {
        index = last_aim_index;
    }
    for (; (path[next(index)]-focus1).norm()+(path[next(index)]-focus2).norm()<2*a; index = next(index));
    //for (; (path[index]-pos).norm()<foresee; index = next(index));
    last_aim_index = index;
    return path [index];
}

double angle_of_2_vector (Vector3d v1, Vector3d v2)
{
    assert (v1.norm()>0);
    assert (v2.norm()>0);
    v1.normalize();
    v2.normalize();
    double cos = v1.dot(v2);
    double sin = v1.cross(v2).z();
    double theta = acos (cos);
    if (sin<0) theta = - theta;
    while (theta>M_PI) theta -= 2*M_PI;
    while (theta<-M_PI) theta += 2*M_PI;
    return theta;
}

vector<double> motion_control::track (Vector3d pos, Vector3d dir)
{
    Vector3d aim = aim_point (pos,dir);
    if ((pos-aim).norm()<0.0001) {
        vector<double> speeds;
        speeds.push_back (0);
        speeds.push_back (0);
        return speeds;
    }
    Vector3d aim_dir = (aim-pos).normalized();
    double theta = angle_of_2_vector (dir,aim_dir);
    if (theta> M_PI_2) theta = M_PI_2;
    if (theta<-M_PI_2) theta =-M_PI_2;
    double R = (aim-pos).norm()/2/cos(M_PI_2-theta);
    double err = speed/R;
    double omega = err * P + (err - errLast) * D;
    errLast = err;
    //double omega = speed/R * P;
    if (fabs(theta)<0.0001) omega = 0;
    if (omega> M_PI_2) omega = M_PI_2;
    if (omega<-M_PI_2) omega =-M_PI_2;
    double left  =-omega*tread/2;
    double right = omega*tread/2;
//    if (theta<M_PI_4 && theta>-M_PI_4) {
//        left += speed;
//        right+= speed;
//    }
//    left = left * (M_PI - fabs(omega)) / M_PI;
//    right = right  * (M_PI - fabs(omega)) / M_PI;

    if (theta < (M_PI/9) && theta > -(M_PI/9))
       isRotation = false;
    if(theta > (M_PI/3) || theta < -(M_PI/3))
       isRotation = true;

    if(!isRotation){
        left += speed;
        right += speed;
        riseRotation += 0.05;
        if(riseRotation > 1.0)
            riseRotation = 1.0;
        left = left * (M_PI - fabs(omega)) / (M_PI) * riseRotation;
        right = right  * (M_PI - fabs(omega)) / (M_PI) * riseRotation;
    } else {
        riseRotation = 0.3;
        double speed_k = fabs(theta) / M_PI_2;
        if(speed_k < 0.5)
            speed_k = 0.5;
        left = left * speed_k;
        right= right * speed_k;
    }
//    left = left * (M_PI - fabs(omega)) / (M_PI);
//    right = right  * (M_PI - fabs(omega)) / (M_PI);


    vector<double> speeds;
    speeds.push_back (left);
    speeds.push_back (right);
    return speeds;
}

void motion_control::getLidarData(vector<Vector3d> lidarList) {
    LOGD("JNI GetLidar succeed");
}













